#include "gimbal.h"
#include "robot_def.h"
#include "servo_motor.h"
#include "message_center.h"
#include "general_def.h"
#include "dji_motor.h"
#include <math.h>
#include "user_lib.h"

#define REDUCTION_RATIO 1.5 * 49

static Publisher_t *gimbal_pub;                   // 云台应用消息发布者(云台反馈给cmd)
static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息

static ServoInstance *gimbal_pitch;
static DJIMotorInstance *gimbal_yaw;

static int8_t init_flag = 1;
static float gimbal_first_angle, pitch_angle;
void GimbalInit()
{
    // 云台电机初始化
    Motor_Init_Config_s yaw_config = {
        .can_init_config = {
            .can_handle = &hcan1,
            .tx_id = 5,
        },
        .controller_param_init_config = {
            .speed_PID = {
                .Kp = 1,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 10000,
                .Improve = PID_ErrorHandle,
            },
            .angle_PID = {
                .Kp = 1,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 10000,
            },
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = SPEED_LOOP,
            .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = M2006,
    };
    gimbal_yaw = DJIMotorInit(&yaw_config);

    // tbd: 云台舵机初始化
    Servo_Init_Config_s pitch_config = {
        .Channel = TIM_CHANNEL_2,
        .htim = &htim1,
        .Servo_Angle_Type = Free_Angle_mode,
        .Servo_type = Servo180,
    };
    gimbal_pitch = ServoInit(&pitch_config);
    Servo_Motor_FreeAngle_Set(gimbal_pitch, 90);
    pitch_angle = 0;

    gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
    gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
}

/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
void GimbalTask()
{
    if (init_flag)
    {
        gimbal_first_angle = gimbal_yaw->measure.total_angle;
        init_flag = 0;
    }

    SubGetMessage(gimbal_sub, &gimbal_cmd_recv);

    // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref
    switch (gimbal_cmd_recv.gimbal_mode)
    {
    // 停止
    case GIMBAL_OFF:
        DJIMotorStop(gimbal_yaw);
        break;
    case GIMBAL_FREE_MODE:
        DJIMotorEnable(gimbal_yaw);
        if (gimbal_yaw->measure.total_angle - gimbal_first_angle <= 180 * REDUCTION_RATIO && gimbal_cmd_recv.yaw == 1)
        {
            DJIMotorOuterLoop(gimbal_yaw, SPEED_LOOP);
            DJIMotorSetRef(gimbal_yaw, 40000 * gimbal_cmd_recv.yaw);
        }
        else if (gimbal_yaw->measure.total_angle - gimbal_first_angle > 0 && gimbal_cmd_recv.yaw == -1)
        {
            DJIMotorOuterLoop(gimbal_yaw, SPEED_LOOP);
            DJIMotorSetRef(gimbal_yaw, 40000 * gimbal_cmd_recv.yaw);
        }
        else
        {
            DJIMotorOuterLoop(gimbal_yaw, ANGLE_LOOP);
            DJIMotorSetRef(gimbal_yaw, gimbal_yaw->measure.total_angle);
        }

        pitch_angle += gimbal_cmd_recv.pitch;
        pitch_angle = abs_limit(pitch_angle, 20);
        if (gimbal_cmd_recv.pitch != 0)
            Servo_Motor_FreeAngle_Set(gimbal_pitch, 0 + pitch_angle);
        else
            ;
        break;
    default:
        break;
    }
    // 推送消息
    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
}